package projects.ufam.nodes.nodeImplementations;

import java.util.ArrayList;
import java.util.HashMap;

import projects.defaultProject.nodes.nodeImplementations.LocalizableNode;
import projects.defaultProject.nodes.timers.MessageTimer;
import projects.defaultProject.nodes.timers.WakeupTimer;
import projects.ufam.CustomGlobal;
import projects.ufam.nodes.messages.OGDCMessage;
import sinalgo.configuration.Configuration;
import sinalgo.nodes.Node;
import sinalgo.nodes.NodeState;
import sinalgo.nodes.Position;
import sinalgo.nodes.messages.Message;
import sinalgo.runtime.Global;
import sinalgo.tools.Tools;

public class OGDCNode extends LocalizableNode {
	double p0, p;
	boolean start = false;
	WakeupTimer t1, t2, t3;
	private HashMap<OGDCNode, Double> ogdcNeighbors = new HashMap<OGDCNode, Double>();
	OGDCNode lastSender = null;
	OGDCMessage lastReceived = null;

	private class UncoveredCrossing {
		public Position p;
		public ArrayList<LocalizableNode> nodes;
	}

	public OGDCNode() {
		p = 1.0 / Tools.getNodeList().size();
		this.switchState(NodeState.WakingUp);
	}

	public void startDensityControl() {
		if (this.isSink()) {
			this.switchState(NodeState.Enabled);
		}
		start = Tools.getRandomNumberGenerator().nextDouble() <= p;
		int t = (int) Global.currentTime;
		t2 = new WakeupTimer();
		t2
				.startRelative(
						Tools.getRandomNumberGenerator().nextInt(10) + 1, this);
		p *= 2;
		p = p > 1.0 ? 1.0 : p;
	}

	public void preStep() {
		if (getState() == NodeState.BatteryDead)
			return;
		super.preStep();
		int t = (int) Global.currentTime;
		if (t % CustomGlobal.densityControlRoundLength == 0) {
			ogdcNeighbors.clear();
			clearTimers(true, true, true);
			lastReceived = null;
			lastSender = null;
			p = 1.0 / Tools.getNodeList().size();
			switchState(NodeState.WakingUp);
			startDensityControl();
		}
	}

	@Override
	public void wakeUp() {
		if (lastReceived == null) {
			if (start) {
				OGDCMessage msg = new OGDCMessage(this, Tools
						.getRandomNumberGenerator().nextDouble()
						* 2.0 * Math.PI);
				MessageTimer t = new MessageTimer(msg);
				t.startRelative(3, this);
			} else {
				startDensityControl();
			}
		} else {
			if (start) {
				this.switchState(NodeState.Enabled);
				lastReceived.setAngle(-1.0);
				this.broadcast(lastReceived);
				clearTimers(true, true, true);
			} else {
				this.switchState(NodeState.WakingUp);
				ogdcNeighbors.clear();
				System.out.println("waking up: " + this.ID);
			}
		}
	}

	double getAngle(LocalizableNode node) {
		// Calculate angle between node and sender
		Position p = getEstimatedPosition();
		Position p2 = node.getEstimatedPosition();
		// invert y-axis
		Position v1 = new Position(p.xCoord - p2.xCoord,
				-(p.yCoord - p2.yCoord), 0.0);
		Position v2 = new Position(1.0, 0.0, 0.0);
		double d1 = Math.sqrt(v1.xCoord * v1.xCoord + v1.yCoord * v1.yCoord);
		double d2 = Math.sqrt(v2.xCoord * v2.xCoord + v2.yCoord * v2.yCoord);
		double dot = dotProduct(v1, v2);
		double beta = Math.acos(dot / (d1 * d2));
		// If angle is negative, convert it to positive
		if (beta < 0) {
			beta = 2.0 * Math.PI + beta;
		}
		if (v1.xCoord == 0) {
			if (v1.yCoord > 0)
				beta = Math.PI / 2.0;
			else
				beta = Math.PI * 2.0 / 3.0;
		} else if (v1.yCoord == 0) {
			if (v1.xCoord > 0)
				beta = 0;
			else
				beta = Math.PI;
		} else {
			beta = Math.atan(v1.yCoord / v1.xCoord);
			if (v1.xCoord > 0 && v1.yCoord > 0) {
				beta += 0.0;
			} else if (v1.xCoord > 0 && v1.yCoord < 0) {
				beta = 2.0 * Math.PI - beta;
			} else if (v1.xCoord < 0 && v1.yCoord < 0) {
				beta += Math.PI;
			} else if (v1.xCoord < 0 && v1.yCoord > 0) {
				beta = Math.PI - beta;
			}
		}
		return beta;
	}

	@Override
	public void handleMessage(Message msg, Node sender) {
		LocalizableNode ln = (LocalizableNode) sender;
		if (msg instanceof OGDCMessage && !this.isFreeNode()
				&& !((LocalizableNode) sender).isFreeNode()) {
			OGDCMessage oMsg = (OGDCMessage) msg.clone();
			if (this.getEstimatedPosition() == null)
				return;
			ogdcNeighbors.put((OGDCNode) sender, this.estimateDistance(ln));
			if (this.estimateDistance(ln) > 2.0 * CustomGlobal.sensorRange)
				return;
			if (this.getState() != NodeState.WakingUp)
				return;
			lastReceived = oMsg;
			if (diskCovered()) {
				// || ln.estimatedDistanceTo(this) < CustomGlobal.sensorRange *
				// 0.5) {
				clearTimers(true, true, true);
				t1 = new WakeupTimer();
				int t = (int) Global.currentTime;
				if (t > CustomGlobal.densityControlRoundLength) {
					t = t % (int) CustomGlobal.densityControlRoundLength;
				}
				t = (int) CustomGlobal.densityControlRoundLength - t;
				if (t == 0)
					t++;
				t1.startRelative(t, this);
				this.switchState(NodeState.StandBy);
				ogdcNeighbors.clear();
				start = false;
				return;
			}
			double alpha = ((OGDCMessage) msg).getAngle();
			UncoveredCrossing uc = getClosestUncoveredCrossing();
			if (alpha == -1) {
				// Case (i) from the paper.
				if (uc.p != null && uc.nodes.size() == 2) {
					// If the crossing was created by the sender of this message
					double dist = ((LocalizableNode) sender)
							.getEstimatedPosition().distanceTo(uc.p);
					// Há incerteza nas interseções..
					if (Math.abs(dist - CustomGlobal.sensorRange) < 1.0) {

						Position[] opt = getOptimalThirdPoint(uc.nodes.get(0)
								.getEstimatedPosition(), uc.nodes.get(1)
								.getEstimatedPosition());
						Position optimal = opt[0]
								.distanceTo(getEstimatedPosition()) < opt[1]
								.distanceTo(getEstimatedPosition()) ? opt[0]
								: opt[1];

						clearTimers(true, true, true);
						if (t1 != null)
							t1.setEnabled(false);
						t1 = new WakeupTimer();
						// Cancel timers; start a tc1 timer
						double d = optimal.distanceTo(this
								.getEstimatedPosition());
						double time = getTime(d);
						time += Tools.getRandomNumberGenerator().nextInt(30);
						lastSender = (OGDCNode) sender;
						t1.setEnabled(true);
						t1.startRelative(1 + (int) time, this);
						start = true;
					} else {
						clearTimers(false, true, true);
					}
				} else
					clearTimers(false, true, true);
			} else {
				// Case (ii) from paper
				if (ogdcNeighbors.containsKey((OGDCNode) ((OGDCMessage) msg)
						.getStartingNode())) {
					// This part seems to be alright. Test with PI/2 worked
					// well.
					if ((OGDCNode) ((OGDCMessage) msg).getStartingNode() == sender) {
						clearTimers(true, true, true);
						if (t2 == null)
							t2 = new WakeupTimer();
						Position sp = ((LocalizableNode) sender)
								.getEstimatedPosition();
						double d = sp.distanceTo(this.getEstimatedPosition());
						double x = Math.cos(alpha) * CustomGlobal.sensorRange
								* Math.sqrt(3.0);
						double y = Math.sin(alpha) * CustomGlobal.sensorRange
								* Math.sqrt(3.0);
						x += sp.xCoord;
						y = sp.yCoord - y;

						Position sp2 = new Position(x, y, 0.0);
						d = sp2.distanceTo(this.getEstimatedPosition());
						double time = getTime(d);
						time += Tools.getRandomNumberGenerator().nextInt(30);
						t2.setEnabled(true);
						t2.startRelative(1 + (int) time, this);
						lastSender = (OGDCNode) sender;
						start = true;
					} else {
						clearTimers(true, false, true);
						// keep tc2 timer
					}
					// Case (iii) from paper
				} else {
					if (getClosestNeighbor() == sender) {
						// Cancel timers; start a tc3 timer
						clearTimers(true, true, true);
						if (t3 == null)
							t3 = new WakeupTimer();
						lastSender = (OGDCNode) sender;
						t3.startRelative(
								CustomGlobal.densityControlRoundLength / 10,
								this);
					} else {
						clearTimers(true, true, false);
						// keep tc3 timer
					}
				}
			}
		} else
			super.handleMessage(msg, sender);
	}

	private double getTime(double d) {
		double time = 100;
		if (d <= 2)
			time = 1.0;
		else if (d <= 4)
			time = 11.0;
		else if (d <= 6)
			time = 21.0;
		else if (d <= 8)
			time = 31.0;
		else
			time = 50;

		// Adiciona o residual ao tempo no controle de densidade
		if (CustomGlobal.useResidualOnDC) {
			double r = getResidual() < 0.01 ? 0.1 : (getResidual() > 500 ? 10
					: getResidual());
			time += r;
			time = time > 50 ? 50 : time;
		}

		return time;
	}

	private double getDeltaAlpha(UncoveredCrossing uc) {
		if (uc.nodes == null)
			return 1;
		if (uc.nodes.size() < 2) {
			return 1;
		}
		Position[] optimal = getOptimalThirdPoint(uc.nodes.get(0)
				.getEstimatedPosition(), uc.nodes.get(1).getEstimatedPosition());
		Position c = null;
		try {
			if (optimal[0].distanceTo(this.getEstimatedPosition()) < optimal[1]
					.distanceTo(this.getEstimatedPosition()))
				c = optimal[0];
			else
				c = optimal[1];
			if (c == null)
				c = optimal[0];
		} catch (Exception e) {
			e.printStackTrace();
		}
		double d = uc.p.distanceTo(this.getEstimatedPosition());
		double d2 = uc.p.distanceTo(c);

		Position v1 = new Position(c.xCoord - uc.p.xCoord, c.yCoord
				- uc.p.yCoord, 0.0);
		Position v2 = new Position(getEstimatedPosition().xCoord - uc.p.xCoord,
				getEstimatedPosition().yCoord - uc.p.yCoord, 0.0);
		double dot = dotProduct(v1, v2);
		return Math.acos(dot / (d * d2));
	}

	public static double dotProduct(Position p1, Position p2) {
		return p1.xCoord * p2.xCoord + p1.yCoord * p2.yCoord + p1.zCoord
				* p2.zCoord;
	}

	private static double[] solveSecondDegreeEquation(double a, double b,
			double c) {
		double[] roots = null;
		double delta = b * b - 4 * a * c;
		if (delta >= 0) {
			roots = new double[2];
			roots[0] = -b + Math.sqrt(delta);
			roots[0] /= 2.0 * a;
			roots[1] = -b - Math.sqrt(delta);
			roots[1] /= 2.0 * a;
		}
		return roots;
	}

	private static Position[] getOptimalThirdPoint(Position p1, Position p2) {
		Position[] result = null;
		double d = p1.distanceTo(p2);
		if (p1.xCoord - p2.xCoord != 0) {
			result = new Position[2];
			double alpha = Math.pow(p1.xCoord, 2) + Math.pow(p1.yCoord, 2)
					- Math.pow(p2.xCoord, 2) - Math.pow(p2.yCoord, 2);
			alpha /= 2.0 * (p1.xCoord - p2.xCoord);
			double beta = p1.yCoord - p2.yCoord;
			beta /= (p1.xCoord - p2.xCoord);

			double b = 2.0 * p1.xCoord * beta - 2.0 * beta * alpha - 2.0
					* p1.yCoord;
			double a = (1.0 + Math.pow(beta, 2));
			double c = Math.pow(p1.xCoord, 2) + Math.pow(p1.yCoord, 2) - 2.0
					* p1.xCoord * alpha + alpha * alpha - d * d;

			double y[] = solveSecondDegreeEquation(a, b, c);
			double x[] = null;
			if (y != null) {
				x = new double[2];
				x[0] = alpha - beta * y[0];
				x[1] = alpha - beta * y[1];
				result[0] = new Position(x[0], y[0], 0.0);
				result[1] = new Position(x[1], y[1], 0.0);
			}
		} else if (p1.yCoord - p2.yCoord != 0) {
			result = new Position[2];
			double alpha = Math.pow(p1.xCoord, 2) + Math.pow(p1.yCoord, 2)
					- Math.pow(p2.xCoord, 2) - Math.pow(p2.yCoord, 2);
			alpha /= 2.0 * (p1.yCoord - p2.yCoord);
			double beta = p1.xCoord - p2.xCoord;
			beta /= (p1.yCoord - p2.yCoord);

			double b = 2.0 * p1.yCoord * beta - 2.0 * beta * alpha - 2.0
					* p1.xCoord;
			double a = (1 + Math.pow(beta, 2));
			double c = Math.pow(p1.yCoord, 2) + Math.pow(p1.xCoord, 2) - 2.0
					* p1.yCoord * alpha + alpha * alpha - d * d;
			double x[] = solveSecondDegreeEquation(a, b, c);
			double y[] = null;
			if (x != null) {
				y = new double[2];
				y[0] = alpha - beta * x[0];
				y[1] = alpha - beta * x[1];
				result[0] = new Position(x[0], y[0], 0.0);
				result[1] = new Position(x[1], y[1], 0.0);
			}
		}
		// There is no point on returning points outside the area. The time out
		// will just take longer.
		if (result != null) {
			for (int i = 0; i < result.length; i++) {
				if (result[i].xCoord < 0)
					result[i].xCoord = 0;
				if (result[i].yCoord < 0)
					result[i].yCoord = 0;
				if (result[i].xCoord > Configuration.dimX)
					result[i].xCoord = Configuration.dimX;
				if (result[i].yCoord > Configuration.dimY)
					result[i].yCoord = Configuration.dimY;
			}
		}
		return result;
	}

	private void clearTimers(boolean b1, boolean b2, boolean b3) {

		if (b1 && t1 != null) {
			((WakeupTimer) t1).setEnabled(false);
		}
		if (b2 && t2 != null) {
			((WakeupTimer) t2).setEnabled(false);
		}
		if (b3 && t3 != null) {
			((WakeupTimer) t3).setEnabled(false);
		}
	}

	private UncoveredCrossing getClosestUncoveredCrossing() {
		HashMap<String, Position> temp = new HashMap<String, Position>();
		HashMap<Position, ArrayList<LocalizableNode>> map = new HashMap<Position, ArrayList<LocalizableNode>>();
		for (Node n1 : ogdcNeighbors.keySet()) {
			if (((LocalizableNode) n1).getEstimatedPosition() == null)
				continue;
			for (Node n2 : ogdcNeighbors.keySet()) {
				if (n2 == n1)
					continue;
				Position[] points = this.circleIntersections(
						(LocalizableNode) n1, CustomGlobal.sensorRange,
						(LocalizableNode) n2, CustomGlobal.sensorRange);
				if (points == null)
					continue;
				if (points[0].xCoord >= 0 && points[0].yCoord >= 0) {
					temp.put(points[0].toString(), points[0]);
					map.put(points[0], new ArrayList<LocalizableNode>());
					map.get(points[0]).add((LocalizableNode) n1);
					map.get(points[0]).add((LocalizableNode) n2);
				}
				if (points[1].xCoord >= 0 && points[1].yCoord >= 0) {
					temp.put(points[1].toString(), points[1]);
					map.put(points[1], new ArrayList<LocalizableNode>());
					map.get(points[1]).add((LocalizableNode) n1);
					map.get(points[1]).add((LocalizableNode) n2);
				}
			}
		}
		double distance = Double.MAX_VALUE;
		UncoveredCrossing result = new UncoveredCrossing();
		for (Position p : map.keySet()) {
			if (p.distanceTo(this.getEstimatedPosition()) < distance) {
				result.p = p;
				result.nodes = map.get(p);
			}
		}
		return result;
	}

	private Node getClosestNeighbor() {
		Node result = null;
		double distance = Double.MAX_VALUE;
		for (Node node : ogdcNeighbors.keySet()) {
			if (ogdcNeighbors.get(node) == null)
				continue;
			if (ogdcNeighbors.get(node).doubleValue() < distance) {
				result = node;
			}
		}
		return result;
	}

	private int calculatePropagationFactor() {
		return 1;
	}

	public boolean diskCovered() {
		if (getEstimatedPosition() == null)
			return false;
		boolean covered = false;
		// Verify if all the ogdcNeighbors cover this disk. This problem's
		// actually pretty tough.
		// Do a spiral from the origin towards the border of the coverage
		// region.
		// If all points on the spiral are covered, the disk is covered.
		double x, y, r;
		r = CustomGlobal.sensorRange;
		int totalCount = 0;
		for (double i = 0; i < 105; i++) {
			x = i * r / 100 * Math.cos(i * 16.0 * Math.PI / 100);
			y = i * r / 100 * Math.sin(i * 16.0 * Math.PI / 100);
			x += this.getEstimatedPosition().xCoord;
			y += this.getEstimatedPosition().yCoord;
			if (x < 0 || y < 0 || x > Configuration.dimX
					|| y > Configuration.dimY){
				totalCount++;
				continue;
			}
			int count = 0;
			for (OGDCNode n : ogdcNeighbors.keySet()) {
				if (n.getEstimatedPosition() == null)
					continue;
				if (n.getEstimatedPosition()
						.distanceTo(new Position(x, y, 0.0)) <= r) {
					// Goes for k-coverage here
					count++;
					if (count == CustomGlobal.kCoverage) {
						break;
					}
				}
			}
			if (count == CustomGlobal.kCoverage)
				totalCount++;
		}
		if (totalCount > 90)
			return true;
		else
			return false;
	}

	public String toString() {
		return "" + this.ID + " " + this.getEstimatedPosition();
	}

}
